#include "../RoverMngrNode/Driver.h"
#include "../utilities/MsgParser.h"
#include "../utilities/Constants.h"
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>
#include <std_msgs/Float32MultiArray.h>

#include <vector>
#include <string>
#include <string.h>

#include "SubsData.h"

using namespace std;

#define MSG_PARAMS_NUM 3 // number of parameters in a message for Rover Module
#define MSG_PARAMS_DELIMITER " " // delimiter of parameters in a message for Rover Module

#define FRONT_LSR_MIN_POINT 40 // start of most-right area to sample at front laser
#define FRONT_LSR_MAX_POINT 50 // end of most-right area to sample at front laser

ros::NodeHandle* node_handler;

vector<SubsData*> subscribers; // container holding each subscribed task

/**
 * Notifies the subscribers that are subscribed to sensor matching given index
 * and sensor's value (distance) is within the subscriber's notification range
 * @param sensor_index index of relevant sensor (see \ref ../utilities/Constants.h)
 * @param distance sensor's current value
 */
void notifySubscriber(int sensor_index, double distance) {

	float min = 0.0, max = 0.0; // hold bounds for each subscriber

	// iterate over all subscribers
	for (unsigned int i = 0 ; i < subscribers.size() ; ++i) {

		// check if current subscriber is subscribed to this sensor
		if (subscribers[i]->isSubscribed(sensor_index)) {

			// get notification range for current subscriber
			subscribers[i]->getRange(sensor_index, min, max);

			// check if should notify
			if (min < distance && distance < max) {
				continue;
			}

			// notify subscriber

			// initialize message
			char buffer[MESSAGE_BUFFER_LENGTH];
			std_msgs::String msg;

			// put the distance to the message
			snprintf(buffer, sizeof(buffer), "%f", distance);

			// set message's data
			msg.data = buffer;

			// publish the message
			subscribers[i]->publishers[sensor_index].publish(msg);
		}
	}
}

/**
 * Publishes front laser's range to the subscribers that are subscribed to it. \n
 * The function publishes the relevant range's section for each subscriber. \n
 * Attention- as lower the index (of scan message) is, it indicates more to the right
 * @param values range values of the front laser
 */
void notifyFrontRange(sensor_msgs::LaserScan::_ranges_type values) {

	float right = 0.0, left = 0.0; // hold bounds for each subscriber

	// iterate over all subscribers
	for (unsigned int i = 0 ; i < subscribers.size() ; ++i) {

		// check if current subscriber is subscribed to this sensor
		if (subscribers[i]->isSubscribed(sensors_names::FRONT_LASER_RANGE)) {

			// get notification range for current subscriber
			subscribers[i]->getRange(sensors_names::FRONT_LASER_RANGE, right, left);
			// set start and end indices
			right = (right > 0) ? right : 0;
			left = (left < 720) ? left : 720 - 1;

			// initialize array
			std_msgs::Float32MultiArray range;
			range.data.clear();

			// add data from right edge to middle position
			for (int j = right ; j < 350 ; ++j) {
				range.data.push_back(values[j]);
			}

			// add data from middle position to left edge
			for (int j = 350 ; j < left; ++j) {
				range.data.push_back(values[j]);
			}

			// publish the message
			subscribers[i]->publishers[sensors_names::FRONT_LASER_RANGE].publish(range);

		}
	}
}

/**
 * Calculates the average among sampled values of front laser sensor: \n
 * consider only the values bounded by indicated range. \n
 * @param values values of the sampled range
 * @param left_range left edge bound
 * @param right_range right edge bound
 * @return average value of the section indicated by given indices
 */
double average(sensor_msgs::LaserScan::_ranges_type values, int left_range, int right_range) {

	// validate range
	if (right_range == left_range) {
		return 0.0;
	}

	// calculate average sample and return result
	double avgDist = 0.0;

	for (int i = left_range ; i <= right_range ; ++i) {
		avgDist += values[i];
	}

	avgDist /= (right_range - left_range+1);
	return avgDist;
}

/**
 * Update subscription according to message. Message format is: \n
 * sensor_index DELIMITER is_activated DELIMITER minimal_distance DELIMITER maximal_distance. \n
 * Sensor's index is set according to \ref ../utilities/Constants.h. \n
 * Activated-flag means if should indeed publish a message (used to allow canceling the subscription). \n
 * Minimal and maximal distance define the notification range
 * @param msg topic message holding subscriber data
 */
void updateSubscriber(const std_msgs::String::ConstPtr& msg) {

	// check error in message
	if (msg->data.empty()) {
		ROS_INFO("EMPTY MESSAGE");
		return;
	}

	// split message by delimiter
	char dataChars[msg->data.length()];
	strcpy(dataChars, msg->data.c_str());

	// store tokens
	vector<string> tokens;
	char* pch = strtok(dataChars,DELIMITER);
	while (pch != NULL) {
		tokens.push_back(pch);
		pch = strtok(NULL, DELIMITER);
	}

	// extract data
	string topic_suffix = tokens[0].c_str();
	short sensor_index = atoi(tokens[1].c_str());
	short isActive = atoi(tokens[2].c_str());

	ROS_INFO("topic_suffix = %s; sensor index= %d, is_active = %d",topic_suffix.c_str(), sensor_index, isActive);

	SubsData* new_subscriber = NULL;

	// find subscriber matching parsed topic suffix
	unsigned int subsPosition = 0;
	for(subsPosition = 0; subsPosition < subscribers.size(); ++subsPosition) {

		if (subscribers[subsPosition]->topic_suffix == topic_suffix) {
			new_subscriber = subscribers[subsPosition];
			break;
		}
	}

	// not found, create a new subscriber only if not de-subscribing
	if (NULL == new_subscriber) {

		if (!isActive) {
			ROS_ERROR("Error- tried to de-activate a non-existing subscriber.");
			return;
		}

		// create subscriber
		new_subscriber = new SubsData(topic_suffix);
		subscribers.push_back(new_subscriber);
		ROS_INFO("updateSubscriber: created a new subscriber");
	}

	// if active, update critical distance
	if (isActive) {

		new_subscriber->subscribe(sensor_index,atof(tokens[3].c_str()),atof(tokens[4].c_str()));
		ROS_INFO("updateSubscriber: updated an existing subscriber");

	} else { // otherwise, cancel subscription for indicated sensor

		ROS_INFO("updateSubscriber: isActive is false.");
		new_subscriber->deSubscribe(sensor_index);

		// if subscriber is not subscribed to any sensor, remove it
		if (!new_subscriber->isSubscribed()) {
			ROS_INFO("subscribers.size() = %d,  subsPosition = %d",subscribers.size(),subsPosition);
			subscribers.erase(subscribers.begin() + subsPosition);
			delete new_subscriber;
			ROS_ERROR("updateSubscriber: deleted subscriber");
		}
	}
}

/**
 * Callback function to receive data from front laser sensor.
 * Sends sampled data to handler of each sensor's part
 * @param scan_msg front laser range message
 */
void frontLaserHandler(const sensor_msgs::LaserScan::ConstPtr& scan_msg) {

	// publish data of sensor's frontal section
   	notifySubscriber(sensors_names::FRONT_LASER,scan_msg->ranges[350]);

	// gather data from right part of sensor and calculate the average
	double avgDist = average(scan_msg->ranges, FRONT_LSR_MIN_POINT, FRONT_LSR_MAX_POINT);

	// publish data of sensor's right section
	notifySubscriber(sensors_names::FRONT_RIGHT_LASER,avgDist-SIDE_SENS_DIFF);

	// publish sensor's scan
	notifyFrontRange(scan_msg->ranges);
}

void backLaserHandler(const sensor_msgs::Range::ConstPtr& scan_msg) {

//		notifySubscriber(sensors_names::BACK_LASER,scan_msg->range);
}

void backUSHandler(const sensor_msgs::Range::ConstPtr& scan_msg) {

//		notifySubscriber(sensors_names::BACK_US,scan_msg->range);
}

void rightUSHandler(const sensor_msgs::Range::ConstPtr& scan_msg) {

//		notifySubscriber(sensors_names::RIGHT_US,scan_msg->range);
}

/**
 * Notifies each of the subscribers of right laser sensor's, that current sensor's value
 * is within the notification range of the subscriber
 * @param scan_msg message holding right laser sesnsor's current value
 */
void rightLaserHandler(const sensor_msgs::Range::ConstPtr& scan_msg) {
	
	notifySubscriber(sensors_names::RIGHT_LASER,scan_msg->range);
}

void leftUSHandler(const sensor_msgs::Range::ConstPtr& scan_msg) {

//	notifySubscriber(sensors_names::LEFT_US,scan_msg->range);
}

void leftLaserHandler(const sensor_msgs::Range::ConstPtr& scan_msg) {

//	notifySubscriber(sensors_names::LEFT_LASER,scan_msg->range);
}

/**
 * Main function of the sensor node: \n
 * subscribes to /Sensors_Module/Subscribers_Channel in order to update subscribers for each sensor,
 * and subscribes to each of the robot's sensors
 */
int main(int argc, char **argv) {

	// initialize system
	ros::init(argc, argv, "SensorsNode");
	node_handler = new ros::NodeHandle();

	// subscribe to sensors module
	int queue_depth = 10;

	// check witch sensors are subscribed
	ros::Subscriber sub_sensors_chanel = node_handler->subscribe("/Sensors_Module/Subscribers_Channel",queue_depth, updateSubscriber);


	ros::Subscriber sub_sensors_front_laser = node_handler->subscribe("scan", 	queue_depth, frontLaserHandler);

	ros::Subscriber sub_sensors_back_ir = node_handler->subscribe("Rangers/Back_IR",  	queue_depth, backLaserHandler);
	
	ros::Subscriber sub_sensors_back_us = node_handler->subscribe("Rangers/Back_US",  	queue_depth, backUSHandler);

	ros::Subscriber sub_sensors_right_ir = node_handler->subscribe("Rangers/Right_IR", 	queue_depth, rightLaserHandler);
	ros::Subscriber sub_sensors_right_us = node_handler->subscribe("Rangers/Right_US", 	queue_depth, rightUSHandler);

	ros::Subscriber sub_sensors_left_ir = node_handler->subscribe("Rangers/Left_IR",  	queue_depth, leftLaserHandler);
	ros::Subscriber sub_sensors_left_us = node_handler->subscribe("Rangers/Left_US",  	queue_depth, leftUSHandler);

	// keep listening to subscribers until ros::shutdown() has been called and is finished
	while (ros::ok()) {
		ros::spinOnce();
	}

	return 0;
}
